
#include<vector>
#include<string.h>
#include<string>
#include<iostream>
#include<fstream>
#include<cstdio>
#include<stdlib.h>
#include <unistd.h>
#include "sri.h"

using namespace std;

SRI::SRI(){
    m_client.ConnectToHost("192.168.0.108",4008);
    bufferDatalen=0;
    dataResvLen=0;
}

SRI::~SRI(){
    m_client.~sriTCPClient();
}

bool SRI::connectTo(const char* address,const int port){
    if(m_client.ConnectToHost(address,port)){
        return true;
    }
    else{
        return false;
    }
}

void SRI::atCommand(string command){
    int len=command.length();
    char sdbuffer[1024];
    memset(sdbuffer,0,sizeof (sdbuffer));
    for(int i=0;i<len;i++){
        sdbuffer[i]=command[i];
    }
    sdbuffer[len]=13;
    sdbuffer[len+1]=10;
    m_client.Send(sdbuffer,strlen(sdbuffer));

    sleep(1);
    if((command!="AT+GOD")&&(command!="AT+GSD")){
        cout<<"send command:"<<sdbuffer<<endl;
        string ack=this->atCommandACK();
        cout<<"接收："<<ack<<endl;
    }
}

string SRI::atCommandACK(){
    this->readACKFromTCP();
    string ack;
    if(strlen(ackData)<4){
        ack="no resv! datalen<4";
        return ack;
    }
    int head=-1;
    for(int i=0;i<strlen(ackData)-4;i++){
        if(ackData[i]=='A'&&ackData[i+1]=='C'&&ackData[i+2]=='K'&&ackData[i+3]=='+'){
            head=i;
        }
    }
    if(head<0){
        ack="no resv! head<0";
        return ack;
    }
    int end=-1;
    for(int i=head;i<strlen(ackData)-1;i++){
        if(ackData[i]==13&&ackData[i+1]==10){
            end=i;
        }
    }
    if(end<0){
        ack="no resv! end<0";
        return ack;
    }
    for(int i=head;i<end+2;i++){
        ack+=ackData[i];
    }
    return ack;
}

vector<float> SRI::readOnce(){
    this->atCommand("AT+GOD");
    vector<float> fm(6,0);
    this->readDataFromTCP();
    this->writeDataToBuffer();
    vector<float> temp=this->parseDataFromBuffer();
    int dellen=temp[6];
    this->delBufferData(dellen);
    for(int i=0;i<6;i++){
        fm[i]=temp[i];
    }
    return fm;
}

vector<vector<float>> SRI::read(){
    this->atCommand("AT+GSD");
    vector<vector<float>> FM(6);
    this->readDataFromTCP();
    this->writeDataToBuffer();
    while(bufferDatalen>0){
        vector<float> temp=this->parseDataFromBuffer();
        int dellen=temp[6];
        this->delBufferData(dellen);
        for(int j=0;j<6;j++){
            FM[j].push_back(temp[j]);
        }
    }
    return FM;

}

//void SRI::readOnceTofile(){
//    ofstream fout("/home/everfocus/testData/M4313data/savevalue"/*,ios_base::out|ios_base::app*/);
//    vector<float> value=this->readOnce();
//    for(int i=0;i<value.size();i++){
//        fout<<value[i]<<' ';
//    }
//    fout<<endl;
//}

//void SRI::readTofile(){
//    ofstream fout("/home/everfocus/testData/M4313data/savewave",ios_base::out|ios_base::app);
//    vector<vector<float>> wave=this->read();
//    for(int i=0;i<wave.size();i++){
//        for(int j=0;j<6;j++){
//            fout<<wave[j][i]<<' ';
//        }
//        fout<<endl;
//    }
//}


void SRI::readDataFromTCP(){
    memset(data,0,sizeof (data));
    dataResvLen=m_client.Recv(data,sizeof (data));
}

void SRI::readACKFromTCP(){
    memset(ackData,0,sizeof (ackData));
    m_client.Recv(ackData,sizeof (ackData));
}

void SRI::writeDataToBuffer(){
    int len=dataResvLen;
    for(int i=0;i<len;i++){
        buffer[i+bufferDatalen]=data[i];
    }
    bufferDatalen+=len;
}

vector<float> SRI::parseDataFromBuffer(){
    int dellen=0;
    vector<float> fm(7,0);//分别对应三个力和三个力矩
    int len=bufferDatalen;
    if(len<29){
        cout<<"数据接收不完整！"<<endl;
        return fm;
    }
    int headindex=this->parseGetHeadIndex();
    if(headindex==-1){
        dellen=len-1;//找不到帧头，删除当前所有数据，保留半个帧头
        fm[6]=dellen;
        cout<<"找不到帧头！"<<endl;
        return fm;
    }
    if(len<headindex+31){//长度不足
        dellen = headindex-1; //删除数据帧头前面的数据
        fm[6]=dellen;
        cout<<"数据长度不足！"<<endl;
        return fm;
    }
    int index=headindex+2;//获得包长度(2字节)
    int framelen=buffer[index]*256+buffer[index+1];
    if(framelen!=27){
        dellen=index;//删除当前帧头
        fm[6]=dellen;
        cout<<"包数据不完整！"<<endl;
        return fm;
    }
    index+=2;
//    int frameNo = buffer[index]*256 + buffer[index+1];//获得包编号
//    cout<<"包编号："<<frameNo<<endl;

    index+=2;
    memcpy(&fm[0],buffer+index,4);

    index+=4;
    memcpy(&fm[1],buffer+index,4);

    index+=4;
    memcpy(&fm[2],buffer+index,4);

    index+=4;
    memcpy(&fm[3],buffer+index,4);

    index+=4;
    memcpy(&fm[4],buffer+index,4);

    index+=4;
    memcpy(&fm[5],buffer+index,4);

    index+=4;
//    int check=buffer[index];//获得奇偶校验
    index+=1;
    dellen=index;//删除当前帧数据
    fm[6]=dellen;
    return fm;

}

int SRI::parseGetHeadIndex(){
    int headindex=-1;
    int len=bufferDatalen;
    for(int i=0;i<len-1;i++){
        if((buffer[i]==170)&&(buffer[i+1]==85)){//搜索数据头帧0xAA 0x55
            headindex=i;
            break;
        }
    }
    return headindex;
}

void SRI::delBufferData(int dellen){
    int len=bufferDatalen;
    len=min(dellen,len);
    if(len<=0){
        return;
    }
    if(len==bufferDatalen){
        bufferDatalen=0;
        memset(buffer,0,sizeof (buffer));
    }
    else if(len<bufferDatalen){
        int newlen=bufferDatalen-len;
        for(int i=0;i<newlen;i++){
            buffer[i]=buffer[i+len];
        }
        for(int j=newlen;j<bufferDatalen;j++){
            buffer[j]=0;
        }
        bufferDatalen=newlen;
    }
}


















